#include <iostream>
#include <vector>
#include <cmath>

using namespace std;

// 定义机器人的初始位置和目标位置
vector<double> start{0, 0};
vector<double> goal{5, 5};

// 定义环境地图和障碍物
vector<vector<double>> obstacles{{2, 2},
                                 {3, 3},
                                 {4, 4}};

// 定义机器人的运动速度和步长
double velocity = 1;
double step_size = 0.1;

// 定义距离函数
double distance(vector<double> point1, vector<double> point2) {
    double dx = point1[0] - point2[0];
    double dy = point1[1] - point2[1];
    return sqrt(dx * dx + dy * dy);
}

// 判断点是否在障碍物内
bool in_obstacle(vector<double> point) {
    for (int i = 0; i < obstacles.size(); i++) {
        double d = distance(point, obstacles[i]);
        if (d < 0.5) {
            return true;
        }
    }
    return false;
}

// BUG算法
vector<vector<double>> bug_algorithm() {
    vector<vector<double>> path;
    vector<double> current = start;
    bool following_wall = false;
    bool clockwise = false;
    while (distance(current, goal) > 0.1) {
        double dx = goal[0] - current[0];
        double dy = goal[1] - current[1];
        double theta = atan2(dy, dx);

        if (!following_wall) {
            // 沿着环境边缘前进
            current[0] += velocity * cos(theta) * step_size;
            current[1] += velocity * sin(theta) * step_size;
        } else {
            // 沿着障碍物边缘前进
            vector<double> left{current[0] + sin(theta), current[1] - cos(theta)};
            vector<double> right{current[0] - sin(theta), current[1] + cos(theta)};
            if (in_obstacle(left) && in_obstacle(right)) {
                // 障碍物包围了机器人，无法绕过
                return path;
            } else if (in_obstacle(left)) {
                current = right;
                clockwise = true;
            } else if (in_obstacle(right)) {
                current = left;
                clockwise = false;
            } else {
                // 找到缺口，从缺口处绕过障碍物
                if (clockwise) {
                    current = left;
                } else {
                    current = right;
                }
            }
        }
        // 判断是否遇到障碍物
        if (in_obstacle(current)) {
            following_wall = true;
            double dx = goal[0] - current[0];
            double dy = goal[1] - current[1];
            theta = atan2(dy, dx);
        }
        path.push_back(current);
    }
    path.push_back(goal);
    return path;
}

int main() {
    vector<vector<double>> path = bug_algorithm();
    for (int i = 0; i < path.size(); i++) {
        cout << path[i][0] << ", " << path[i][1] << endl;
    }
    return 0;
}